import numpy as np
try:
    from calculate_line_angle import calculate_line_angle
    from dubins import dubins_path_planning
    from is_all_points_inside_poly import is_all_points_inside_poly
    from move_all_points import move_all_points
except:
    from .calculate_line_angle import calculate_line_angle
    from .dubins import dubins_path_planning
    from .is_all_points_inside_poly import is_all_points_inside_poly
    from .move_all_points import move_all_points


def Wangfu_turning_operation(local_intersection_points_path_0, first_point, second_point, nested_turning_area_width, local_field_boundary_poly, curvature, TRACTOR_WIDTH):

    extra_width = 0.02 # 预留边界余量
    local_field_boundary_poly_buffered = local_field_boundary_poly.buffer( -(TRACTOR_WIDTH/2+extra_width), join_style=2, mitre_limit=5.0)


    # 第一条边界有2个点
    if len(local_intersection_points_path_0)==2:
        length_0 = np.sqrt( abs(local_intersection_points_path_0[0][0]-second_point[0])**2 + abs(local_intersection_points_path_0[0][1]-second_point[1])**2 )
        length_1 = np.sqrt( abs(local_intersection_points_path_0[1][0]-second_point[0])**2 + abs(local_intersection_points_path_0[1][1]-second_point[1])**2 )

        if length_0<length_1:
            third_point = local_intersection_points_path_0[0]
            fourth_point = local_intersection_points_path_0[1]
        else:
            third_point = local_intersection_points_path_0[1]
            fourth_point = local_intersection_points_path_0[0]

        # 出入角度
        out_yaw = calculate_line_angle(first_point[0], first_point[1], second_point[0], second_point[1], False)  # [rad]
        in_yaw = calculate_line_angle(third_point[0], third_point[1], fourth_point[0], fourth_point[1], False)

        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(second_point[0],second_point[1],out_yaw,third_point[0],third_point[1],in_yaw,curvature)

        each_move_distance = 0.05 #单位是米
        each_move_direction = [first_point[0]-second_point[0], first_point[1]-second_point[1]]/np.sqrt(abs(second_point[0]-first_point[0])**2+abs(second_point[1]-first_point[1])**2)

        # 如果移动后的路径曲线仍然有点在多边形之外
        while not is_all_points_inside_poly(local_path_x, local_path_y, local_field_boundary_poly_buffered ):
            # 则继续移动
            moved_local_path_x, moved_local_path_y = move_all_points(local_path_x, local_path_y, each_move_direction, each_move_distance)
            local_path_x = moved_local_path_x
            local_path_y = moved_local_path_y


    return third_point, fourth_point, local_path_x, local_path_y, path_yaw, mode, lengths